﻿//-------------------------------------------------------------------
//	Copyright (c) 2010-2014  Zhirnov Andrey
//	This file is part of the "UX Standard Lib" project.
//	See copyright notice in "UX_STL_Ext.h".
//-------------------------------------------------------------------

#ifndef _UXS_TRANSFORM_H
#define _UXS_TRANSFORM_H

#pragma once

#include "MathTypesExt.h"

namespace UX_STL
{
namespace UXMathExt
{

	//
	// Transform
	//

	template <typename T>
	struct TTransform
	{
	public:
		typedef TVec<T,3>		vec_t;
		typedef TQuaternion<T>	quat_t;
		typedef TMatrix<T,4,4>	mat_t;
		typedef TTransform<T>	Self;
			
	private:
		quat_t		_qOrient;
		vec_t		_vPos;

	public:
		TTransform() {}
		TTransform(const Self &tr);
		TTransform(const mat_t &mat);
		TTransform(const T *pos, const T *orient);
		TTransform(const vec_t &pos, const quat_t &orient);

		vec_t		 &	Position()									{ return _vPos; }
		quat_t		 &	Orientation()								{ return _qOrient; }

		const vec_t	 &	Position()		const						{ return _vPos; }
		const quat_t &	Orientation()	const						{ return _qOrient; }

		void			Set(const vec_t &pos, const quat_t &orient)	{ _vPos = pos; _qOrient = orient; }
		
		Self	operator - ()			const						{ return GetInverse(); }

		Self & operator += (const Self &right);
		Self & operator -= (const Self &right);

		Self	operator +  (const Self &right)	const;
		Self	operator -  (const Self &right)	const;

		bool	operator == (const Self &right)	const;
		bool	operator != (const Self &right)	const;

		Self &	Move(const vec_t &delta);
		Self & Rotate(const quat_t &delta);

		Self &	Inverse();
		Self	GetInverse()	const;
		
		mat_t	GetMatrix()		const;

		vec_t	GetGlobalVector(const vec_t &sLocal)	const;
		vec_t	GetLocalVector(const vec_t &sGlobal)	const;

		vec_t	GetGlobalPos(const vec_t &sLocal)		const;
		vec_t	GetLocalPos(const vec_t &sGlobal)		const;

		
		// free transform (смещение не зависит от вращения) //
		Self &	AddAssignF(const Self &right);
		Self &	SubAssignF(const Self &right);
		Self	AddF(const Self &right) const;
		Self	SubF(const Self &right) const;
		Self &	MoveF(const vec_t &delta);
		Self &	InverseF();
		Self	GetInverseF() const;


		template <typename T2>
		TTransform<T2>	Convert() const;

		string			ToString()	const;
	};


	
	
	template <typename T>
	inline TTransform<T>::TTransform(const T *pos, const T *orient): _vPos(pos), _qOrient(orient)
	{}
	

	template <typename T>
	inline TTransform<T>::TTransform(const TTransform &tr): _vPos(tr._vPos), _qOrient(tr._qOrient)
	{}
	

	template <typename T>
	inline TTransform<T>::TTransform(const TVec<T,3> &pos, const TQuaternion<T> &orient): _vPos(pos), _qOrient(orient)
	{}


	template <typename T>
	inline TTransform<T>::TTransform(const TMatrix<T,4,4> &mat): _vPos( mat.GetTranslation() ), _qOrient( mat )
	{}

	
	
	template <typename T>
	inline TTransform<T> & TTransform<T>::operator += (const TTransform<T> &right)
	{
		_vPos	 += _qOrient * right._vPos;
		_qOrient *= right._qOrient;
		return *this;
	}

	
	template <typename T>
	inline TTransform<T> & TTransform<T>::operator -= (const TTransform<T> &right)
	{
		return *this += right.GetInverse();
	}

	
	template <typename T>
	inline TTransform<T> TTransform<T>::operator + (const TTransform<T> &right) const
	{
		return TTransform<T>( *this ) += right;
	}

	
	template <typename T>
	inline TTransform<T> TTransform<T>::operator - (const TTransform<T> &right) const
	{
		return TTransform<T>( *this ) -= right;
	}

		
	template <typename T>
	inline bool TTransform<T>::operator == (const TTransform<T> &right) const
	{
		return ( _vPos == right._vPos and _qOrient == right._qOrient );
	}
	
	
	template <typename T>
	inline bool TTransform<T>::operator != (const TTransform<T> &right) const
	{
		return not ( *this == right );
	}


	template <typename T>
	inline TMatrix<T,4,4> TTransform<T>::GetMatrix() const
	{
		TMatrix<T,4,4>	m = TMatrix<T,4,4>::FromQuat( Orientation() );
		m.Translation() = _vPos;
		return m;
	}


	template <typename T>
	inline TTransform<T> & TTransform<T>::Inverse()
	{
		_qOrient.Inverse();
		_vPos = _qOrient * -_vPos;
		return *this;
	}


	template <typename T>
	inline TTransform<T> TTransform<T>::GetInverse() const
	{
		return TTransform( *this ).Inverse();
	}
	

	template <typename T>
	inline TTransform<T> & TTransform<T>::Move(const TVec<T,3> &delta)
	{
		_vPos += _qOrient * delta;
		return *this;
	}

	
	template <typename T>
	inline TTransform<T> & TTransform<T>::Rotate(const TQuaternion<T> &delta)
	{
		_qOrient *= delta;
		return *this;
	}

		
	template <typename T>
	inline TTransform<T> & TTransform<T>::AddAssignF(const TTransform<T> &right)
	{
		_vPos	 += right._vPos;
		_qOrient *= right._qOrient;
		return *this;
	}

		
	template <typename T>
	inline TTransform<T> & TTransform<T>::SubAssignF(const TTransform<T> &right)
	{
		return AddAssignF( right.GetInverse() );
	}
	
		
	template <typename T>
	inline TTransform<T> TTransform<T>::AddF(const TTransform<T> &right) const
	{
		return TTransform<T>( *this ).AddAssingF( right );
	}

		
	template <typename T>
	inline TTransform<T> TTransform<T>::SubF(const TTransform<T> &right) const
	{
		return TTransform<T>( *this ).SubAssignF( right );
	}

		
	template <typename T>
	inline TTransform<T> & TTransform<T>::MoveF(const TVec<T,3> &delta)
	{
		_vPos += delta;
		return *this;
	}

		
	template <typename T>
	inline TTransform<T> & TTransform<T>::InverseF()
	{
		_qOrient.Inverse();
		_vPos = -_vPos;
		return *this;
	}
	

	template <typename T>
	inline TTransform<T> TTransform<T>::GetInverseF() const
	{
		return TTransform( *this ).InverseF();
	}


	template <typename T>
	inline TVec<T,3> TTransform<T>::GetGlobalVector(const TVec<T,3> &sLocal) const
	{
		return ( _qOrient * sLocal );
	}

		
	template <typename T>
	inline TVec<T,3> TTransform<T>::GetGlobalPos(const TVec<T,3> &sLocal) const
	{
		return ( _qOrient * sLocal ) + _vPos;
	}

		
	template <typename T>
	inline TVec<T,3> TTransform<T>::GetLocalVector(const TVec<T,3> &sGlobal) const
	{
		return ( _qOrient.GetInverse() * sGlobal );
	}

		
	template <typename T>
	inline TVec<T,3> TTransform<T>::GetLocalPos(const TVec<T,3> &sGlobal) const
	{
		return _qOrient.GetInverse() * ( sGlobal - _vPos );
	}

	
	template <typename T>
	template <typename T2>
	inline TTransform<T2> TTransform<T>::Convert() const
	{
		return TTransform<T2>( _vPos.Convert<T2>(), _qOrient.Convert<T2>() );
	}

	
	template <typename T>
	inline string TTransform<T>::ToString() const
	{
		return (string("pos( ") << _vPos.ToStringFull() << " ), orient( " << _qOrient.ToStringFull() << " )");
	}

//-------------------------------------------------------------------

}	// UXMathExt
}	// UX_STL

#endif	// _UXS_TRANSFORM_H